﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WiimoteLib;

namespace IndoorPositionFinder
{
    /// <summary>
    /// This class represents the caracteristics associated
    /// with a InfraredSensor related with the room. In this case
    /// the wiimote is a type of infraredSensor
    /// </summary>
    public class RoomWiimote
    {
        private float length = 0f; // Sensor length in meters.
        private float angle; // angle in radians generated between the sensor and the right wall.
        private static float openingAngle = 22f*(float)Math.PI/180.0f; // Maximum angle viewed by the sensor. EXPERIMENTAL VALUE
        private PointF position; // position where the wiimote is put relative to the room
        private int direction; // direction where the wiimote is pointing

        public RoomWiimote(PointF position, float angle, int direction)
        {
            this.angle = angle*(float)Math.PI/180.0f;
            this.position = position;
            this.direction = direction;
        }
        #region Getters & Setters
        public float Length { get; set; }
        public float Angle { get; set; }
        /// <summary>
        /// Angle between the Wiimote Normal Vector and the last point viewed by the Wiimote
        /// </summary>
        public static float OpeningAngle { get { return openingAngle;} }
        public PointF Position { get { return position; } set { position = value; } }
        public int Direction { get { return direction; } set { direction = value; } }
        #endregion
        /// <summary>
        /// Calculate the angle used to triangulate the user position
        /// </summary>
        /// <param name="pointAngle">Angle between the right visibility border for the wiimote and the vector position</param>
        /// <returns>the angle between the right wall and the wiimote</returns>
        public float pointAngle(float pointAngle)
        {
            return (this.angle - openingAngle + pointAngle);
        }
        /// <summary>
        /// Calculate the length of the sides from the triangle required to triangulate the user position 
        /// </summary>
        /// <param name="cornerPoints"></param>
        /// <returns>Point containing the length of the sides required to triangulate</returns>
        public PointF triangulationSides(PointF[] cornerPoints)
        {
            PointF triangulationSides = new PointF();
            switch (direction)
            {
                case 1:
                    triangulationSides.X = -1;
                    triangulationSides.Y = -1;
                    foreach(PointF point in cornerPoints)
                    {
                        if(point.X > triangulationSides.X)
                            triangulationSides.X = point.X;
                        if (point.Y > triangulationSides.Y)
                            triangulationSides.Y = point.Y;
                    }
                    break;
                case 2:
                    triangulationSides.X = -1;
                    triangulationSides.Y = 0;
                    foreach(PointF point in cornerPoints)
                    {
                        if(point.X > triangulationSides.X)
                            triangulationSides.X = point.X;
                    }
                    break;
                case 3:
                    triangulationSides.X = 0;
                    triangulationSides.Y = 0;
                    break;
                case 4:
                    triangulationSides.X = 0;
                    triangulationSides.Y = -1;
                    foreach(PointF point in cornerPoints)
                    {
                        if (point.Y > triangulationSides.Y)
                            triangulationSides.Y = point.Y;
                    }
                    break;
            }
            triangulationSides.X = Math.Abs(triangulationSides.X - this.position.X);
            triangulationSides.Y = Math.Abs(triangulationSides.Y - this.position.Y);
            return triangulationSides;
        }
        public float separatorAngle(PointF sides)
        {
            float separatorAngle = .0f;
            switch (direction % 2)
            {
                // Direction 2 or 4
                case 0:
                    separatorAngle = (float)Math.Atan((double)sides.X / (double)sides.Y);
                    break;
                // Direction 1 or 3
                case 1:
                    separatorAngle = (float)Math.Atan((double)sides.Y / (double)sides.X);                
                    break;
            }
            return separatorAngle;
        }
        /// <summary>
        /// Calculate the coordinates associated with the projection of the user position in the
        /// room circumscribed quad.
        /// </summary>
        /// <param name="roomPoints">Room corner points</param>
        /// <param name="pointAngle">Position angle calculated by the wiimote</param>
        /// <returns></returns>
        public PointF projectionPoint(PointF[] roomPoints, float pointAngle)
        {
            // with the previous angle we can obtain the angle required to triangulate the userPosition
            float triangulationAngle = this.pointAngle(pointAngle);
            // Obtaining the virtual point in which direction the wiimote is pointing
            PointF triangleSides = this.triangulationSides(roomPoints);
            // calculating the section separator angle associated with the wiimote;
            float angleSeparator = this.separatorAngle(triangleSides);
            PointF projectionPoint = new PointF();
            switch (direction % 2)
            {
                // Direction 2 or 4
                case 0:
                    // if the point is projected in the right wall
                    // watching it from the wiimote position and direction
                    if (triangulationAngle <= angleSeparator)
                    {
                        projectionPoint.X = triangleSides.Y * (float)Math.Tan((double)triangulationAngle);
                        projectionPoint.Y = triangleSides.Y;
                    }
                    // otherwise
                    else
                    {
                        projectionPoint.X = triangleSides.X;
                        projectionPoint.Y = triangleSides.X / (float)Math.Tan((double)triangulationAngle);

                    }
                    // But the Wiimote point is not the (0,0) so we need to update the
                    // projection point value associated with the real (0,0)
                    if (direction == 2)
                    {
                        projectionPoint.X += this.position.X;
                        projectionPoint.Y = this.position.Y - projectionPoint.Y;
                    }
                    if (direction == 4)
                    {
                        projectionPoint.X = this.position.X - projectionPoint.X;
                        projectionPoint.Y += this.position.Y;
                    }
                    break;
                // Direction 1 or 3
                case 1:
                    // if the point is projected in the right wall
                    // watching it from the wiimote position and direction
                    if (triangulationAngle <= angleSeparator)
                    {
                        projectionPoint.X = triangleSides.X;
                        projectionPoint.Y = triangleSides.X * (float)Math.Tan((double)triangulationAngle);
                    }
                    // otherwise
                    else
                    {
                        projectionPoint.X = triangleSides.Y / (float)Math.Tan((double)triangulationAngle);
                        projectionPoint.Y = triangleSides.Y;
                    }
                    // But the Wiimote point is not the (0,0) so we need to update the
                    // projection point value associated with the real (0,0)
                    if (direction == 1)
                    {
                        projectionPoint.X += this.position.X;
                        projectionPoint.Y += this.position.Y;                        
                    }
                    if (direction == 3)
                    {
                        projectionPoint.X = this.position.X - projectionPoint.X;
                        projectionPoint.Y = this.position.Y - projectionPoint.Y;
                        
                    }
                    break;
            }
            return projectionPoint;
        }
    }
}
